/*
 * @Author: your name
 * @Date: 2021-07-17 10:58:36
 * @LastEditTime: 2021-10-12 11:24:14
 * @LastEditors: Mcfly
 * @Description: In User Settings Edit
 * @FilePath: \FTTfly\Ctrl\AttiCtrl.c
 */

#include "AttiCtrl.h"
#include "stdlib.h"
#include "WP_PWM.h"
#include "Throttle.h"
#include "ti_system.h"
#include "VirtualOscTx.h"

AttiCtrl *AttiCtrl_Init(void)
{
    AttiCtrl *attiCtrl = (AttiCtrl *)malloc(sizeof(AttiCtrl));
    attiCtrl->deltaT = 1 / 200.0f; //TODO: 修改deltaT
    //TODO: test
    attiCtrl->PIDpos[PARAM_ROLL] = *initPID(0.008, 0.0f, 0, 15, attiCtrl->deltaT);
    attiCtrl->PIDpos[PARAM_PITCH] = *initPID(0.008, 0.0f, 0, 15, attiCtrl->deltaT);
    attiCtrl->PIDpos[PARAM_YAW] = *initPID(0, 0, 0, 100, attiCtrl->deltaT);
    attiCtrl->PIDomega[PARAM_ROLL] = *initPID(3187.0f, 12990.0f, 85.0f, 300, attiCtrl->deltaT);
    attiCtrl->PIDomega[PARAM_PITCH] = *initPID(3187.0f, 12990.0f, 85.0f, 300, attiCtrl->deltaT);
    attiCtrl->PIDomega[PARAM_YAW] = *initPID(1350.0f, 7200.0f, 99.0f, 20, attiCtrl->deltaT);
    return attiCtrl;
}

float omegaOut[3];
bool flying;
float lastW = 0;
void AttiCtrl_UpdateFrame(AttiCtrl *attiCtrl, Atti *nowAtti, Atti *expectAtti, float throttle)
{
    if (throttle == 0)
    {
        flying = 0;
        for (int i = 0; i < 3; i++)
        {
            attiCtrl->PIDpos[i].integral = 0;
            attiCtrl->PIDomega[i].integral = 0;
        }
        PWM_Output(0, THROTTLE_MIN);
        PWM_Output(1, THROTTLE_MIN);
        PWM_Output(2, THROTTLE_MIN);
        PWM_Output(3, THROTTLE_MIN);
        return;
    }
    float expectOmega[3];
    float errorPos[3] = {expectAtti->roll - nowAtti->roll,
                         expectAtti->pitch - nowAtti->pitch,
                         expectAtti->yaw - nowAtti->yaw};
    //PIDctrler_SetKp(&attiCtrl->PIDpos[PARAM_PITCH], throttle * 0.001f);

    for (int i = 0; i < 3; i++)
        expectOmega[i] = PIDProcess(&attiCtrl->PIDpos[i], errorPos[i]);
    float errorOmega[3] = {expectOmega[PARAM_ROLL] - nowAtti->gy,
                           expectOmega[PARAM_PITCH] - nowAtti->gx,
                           expectAtti->gz - nowAtti->gz};
    // float errorOmega[3] = {0 - nowAtti->gy,
    //                      expectAtti->pitch - nowAtti->gx,
    //                       0 - nowAtti->gz};
    //PIDctrler_SetKd(&attiCtrl->PIDomega[PARAM_PITCH], throttle * 100.0f);
    for (int i = 0; i < 3; i++)
        omegaOut[i] = PIDProcess(&attiCtrl->PIDomega[i], errorOmega[i]);

	if(expectAtti->w - lastW < 20)
	{
		lastW = 0.8f * expectAtti->w + 0.2f * lastW;
	} else if(expectAtti->w - lastW < 50)
	{
		lastW = 0.6f * expectAtti->w + 0.4f * lastW;
	} else if(expectAtti->w - lastW < 75)
	{
		lastW = 0.3f * expectAtti->w + 0.7f * lastW;
	} else if(expectAtti->w - lastW < 100)
	{
		lastW = 0.15f * expectAtti->w + 0.85f * lastW;
	} else if(expectAtti->w - lastW < 150)
	{
		lastW = 0.04f * expectAtti->w + 0.96f * lastW;
	} 
	expectAtti->w = lastW;
    //油门控制逻辑
    if (lastW < 690 && !flying) //稳态：没起飞时，高阈值
    {
        for (int i = 0; i < 3; i++)
        {
            attiCtrl->PIDpos[i].integral = 0;
            attiCtrl->PIDomega[i].integral = 0;
        }
        PWM_Output(0, (uint16_t)MIN(THROTTLE_MIN + lastW, THROTTLE_MAX));
        PWM_Output(1, (uint16_t)MIN(THROTTLE_MIN + lastW, THROTTLE_MAX));
        PWM_Output(2, (uint16_t)MIN(THROTTLE_MIN + lastW, THROTTLE_MAX));
        PWM_Output(3, (uint16_t)MIN(THROTTLE_MIN + lastW, THROTTLE_MAX));
    }
    else if (lastW < 200 && flying)
    { //暂态：起飞时，低阈值
        flying = 0;
        for (int i = 0; i < 3; i++)
        {
            attiCtrl->PIDpos[i].integral = 0;
            attiCtrl->PIDomega[i].integral = 0;
        }
        PWM_Output(0, (uint16_t)MIN(THROTTLE_MIN + lastW, THROTTLE_MAX));
        PWM_Output(1, (uint16_t)MIN(THROTTLE_MIN + lastW, THROTTLE_MAX));
        PWM_Output(2, (uint16_t)MIN(THROTTLE_MIN + lastW, THROTTLE_MAX));
        PWM_Output(3, (uint16_t)MIN(THROTTLE_MIN + lastW, THROTTLE_MAX));
    }
    else if (lastW >= 690 && !flying)
    { //暂态：没起飞时，高阈值
        flying = 1;
        PWM_Output(0, (uint16_t)MIN(THROTTLE_MIN + lastW + MAX(-omegaOut[PARAM_ROLL] + omegaOut[PARAM_PITCH] - omegaOut[PARAM_YAW], -200), THROTTLE_MAX));
        PWM_Output(1, (uint16_t)MIN(THROTTLE_MIN + lastW + MAX(+omegaOut[PARAM_ROLL] + omegaOut[PARAM_PITCH] + omegaOut[PARAM_YAW], -200), THROTTLE_MAX));
        PWM_Output(2, (uint16_t)MIN(THROTTLE_MIN + lastW + MAX(+omegaOut[PARAM_ROLL] - omegaOut[PARAM_PITCH] - omegaOut[PARAM_YAW], -200), THROTTLE_MAX));
        PWM_Output(3, (uint16_t)MIN(THROTTLE_MIN + lastW + MAX(-omegaOut[PARAM_ROLL] - omegaOut[PARAM_PITCH] + omegaOut[PARAM_YAW], -200), THROTTLE_MAX));
    }
    else if (lastW >= 200 && flying)
    { //稳态：起飞时，低阈值
        PWM_Output(0, (uint16_t)MIN(THROTTLE_MIN + lastW + MAX(-omegaOut[PARAM_ROLL] + omegaOut[PARAM_PITCH] - omegaOut[PARAM_YAW], -200), THROTTLE_MAX));
        PWM_Output(1, (uint16_t)MIN(THROTTLE_MIN + lastW + MAX(+omegaOut[PARAM_ROLL] + omegaOut[PARAM_PITCH] + omegaOut[PARAM_YAW], -200), THROTTLE_MAX));
        PWM_Output(2, (uint16_t)MIN(THROTTLE_MIN + lastW + MAX(+omegaOut[PARAM_ROLL] - omegaOut[PARAM_PITCH] - omegaOut[PARAM_YAW], -200), THROTTLE_MAX));
        PWM_Output(3, (uint16_t)MIN(THROTTLE_MIN + lastW + MAX(-omegaOut[PARAM_ROLL] - omegaOut[PARAM_PITCH] + omegaOut[PARAM_YAW], -200), THROTTLE_MAX));
    }
}
